package javax.robotics.engine.robots;

/*
 * DHRobot3D.java -
 * Copyright (c) 2005 Carmine Lia
 * e-mail:      carmine.lia@libero.it
 * web-address: http://www.intermedia.sa.it/lia

 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

import java.io.File;
import java.io.IOException;
import java.io.InputStream;
import java.net.URI;

import javax.Robotics.vecmath.Point3d;
import javax.Robotics.vecmath.RMatrix4d;
import javax.media.j3d.Appearance;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.Material;
import javax.media.j3d.PolygonAttributes;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.robotics.j3d.Axis;
import javax.robotics.j3d.ColorConstants;
import javax.robotics.j3d.Cylinder;
import javax.robotics.vecmath.Matrix;
import javax.robotics.vecmath.RVector;
import javax.robotics.vecmath.RVector3d;

import javolution.util.FastTable;

import org.apache.xmlbeans.XmlException;

import com.sun.j3d.utils.geometry.Sphere;

/**
 * <p>
 * This class implements graphic 3D, kinematics and dynamics model of a robot
 * manipulator based on Denavit-Hartenberg table.
 * </p>
 * 
 * @author <a href="http://www.intermedia.sa.it/lia/">Carmine Lia</a>
 * @since 1.0.3
 * @version 11/11/2005
 */
public class DHRobot3D extends DHRobot implements Robot3D
{
    /**
     * 
     */
    private static final long serialVersionUID = 3256727294520342068L;
    private static final short MAX_DOF = 6;
        
    private BranchGroup bgRobot;
    private final FastTable<Transform3D> tsGiuntiRef = new FastTable<Transform3D>(MAX_DOF);
    private final FastTable<TransformGroup> tgGiunti = new FastTable<TransformGroup>(MAX_DOF);
    private final FastTable<Sphere> shapeGiunti = new FastTable<Sphere>(MAX_DOF);
    
    // parametri della tabella DH
    private double[] a = new double[MAX_DOF];
    private double[] d = new double[MAX_DOF];
    private double[] alfa = new double[MAX_DOF];
    private double[] teta = new double[MAX_DOF];
    private boolean[] tipoGiunto = new boolean[MAX_DOF];
    private boolean[] immobile = new boolean[MAX_DOF];

    private final int nLinks;

    /**
     * Robot constructor with init parameter.
     * 
     * @param nLinks
     *            number of robot links
     * @param initRobot
     *            vector of robot parameter; dimension: nLinks x 23
     * @see DHRobot#DHRobot(int, double[])
     */
    public DHRobot3D(int nLinks, double[] initRobot)
    {
        super(nLinks, initRobot);
        this.nLinks = nLinks;
        initDHTable(new Matrix(nLinks, INITROBOT_DIM + INITMOTOR_DIM, initRobot));
        create3DModel();
    }
    /**
     * Robot constructor with init parameter and motor parameter.
     * 
     * @param nLinks
     *            number of links or robot
     * @param initRobot
     *            vector of robot parameter; dimension: nLinks x 19
     * @param initMotor
     *            vector of motor robot parameter; dimension: nLinks x 4
     * 
     * @see DHRobot#DHRobot(int, double[], double[])
     */
    public DHRobot3D(int nLinks, double[] initRobot, double[] initMotor)
    {
        super(nLinks, initRobot, initMotor);
        this.nLinks = nLinks;
        initDHTable(new Matrix(nLinks, INITROBOT_DIM, initRobot));
        create3DModel();
    }
    /**
     * Robot constructor with URI of xml file parameter.
     * The URI can be an URL or a file path.
     * @param uri
     *            xml configuration file URI of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot3D(URI uri) throws XmlException, IOException
    {
        super(uri);
        this.nLinks = getRobotConfiguration().getNLinks();
        initDHTable();
        create3DModel();
      
                
    }
    /**
     * Robot constructor with xml file parameter.
     * 
     * @param fileName
     *            xml configuration file name of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot3D(String fileName) throws XmlException, IOException
    {
        this(new File(fileName));
    }
    
    /**
     * Robot constructor with xml file parameter.
     * 
     * @param fileName
     *            xml configuration file name of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot3D(File fileName) throws XmlException, IOException
    {
        super(fileName);
        this.nLinks = getRobotConfiguration().getNLinks();
        initDHTable();
        create3DModel();
    }
    
    /**
     * Robot constructor with xml file parameter as input stream.
     * 
     * @param fileName
     *            xml configuration file name of robot parameter
     * @throws XmlException
     * @throws IOException
     */
    public DHRobot3D(InputStream fileName) throws XmlException, IOException
    {
        super(fileName);
        this.nLinks = getRobotConfiguration().getNLinks();
        initDHTable();
        create3DModel();
    }
    /* (non-Javadoc)
     * @see javax.robotics.engine.robots.Robot3D#isJointPrismatic(int)
     */
    public final boolean isJointPrismatic(int i)
    {
        return this.tipoGiunto[i - 1];
    }
    
    public boolean isJointImmobile(int i)
    {
        return immobile[i - 1];
    }
    
    public final BranchGroup getModel3D()
    {
        return bgRobot;
    }

    public final void setQ3DOnly(RVector q)
    {
        for (int i = 0; i < nLinks; i++)
            setJointPosition(i, q.getElement(i));
    }

    public final void setQ3DOnly(double[] q)
    {

        for (int i = 0; i < nLinks; i++)
            setJointPosition(i, q[i]);

    }

    public final void setQ3DOnly(double q, int i)
    {
        setJointPosition(i - 1, q);

    }

    public final void setQ3D(RVector q)
    {
        final double[] q_ = new double[q.getSize()];
        q.get(q_);
        setQ(getID(), q_);
        setQ3DOnly(q);
    }

    public final void setQ3D(double[] q)
    {
        setQ(getID(), q);
        setQ3DOnly(q);

    }

    public final void setQ3D(double q, int i)
    {
        setQ(getID(), q, i);
        setQ3DOnly(q, i);
    }

    // **** private method ****//

    /**
     * Init DH table
     * 
     * @param initRobot
     */
    private final void initDHTable(Matrix initRobot)
    {
        for (int i = 0; i < nLinks; i++)
        {
            a[i] = initRobot.getElement(i, 3);
            d[i] = initRobot.getElement(i, 2);
            alfa[i] = initRobot.getElement(i, 4);
            teta[i] = initRobot.getElement(i, 1);
            tipoGiunto[i] = (initRobot.getElement(i, 0) == 0)? REVOLUTE:PRISMATIC;
            if(initRobot.getNumCol() == INITMOTOR_DIM + INITROBOT_DIM) 
                immobile[i] = (initRobot.getElement(i, 22) == 0)? false:true;
            /*
            if((initRobot.getElement(i, 0) == 0))
                tipoGiunto[i] = REVOLUTE;
            else
                tipoGiunto[i] = PRISMATIC;
                */
        }
               
    }
    
    private final void initDHTable()
    {
        a = getRobotConfiguration().getA();
        d = getRobotConfiguration().getD();
        alfa = getRobotConfiguration().getAlfa();
        teta = getRobotConfiguration().getTeta();
        tipoGiunto = getRobotConfiguration().getType();
        immobile = getRobotConfiguration().getImmobile();
    }
    
    private final Cylinder createZArm(Point3d pos, double length,  Appearance app)
    {
        final Cylinder zarm = new Cylinder(pos, 0.06,
                length, Cylinder.TOP_BOTTOM);
        zarm.setAppearance(app);
        return zarm;

    }

    private Cylinder createXArm(double length, Appearance app)
    {
        final Cylinder xarm = new Cylinder(new Point3d(), 0.07, 0.06, length,
                Cylinder.TOP, Cylinder.X);
        xarm.setAppearance(app);
        return xarm;

    }

    private Sphere createJoint(float radius, Appearance app)
    {

        return new Sphere(radius, Sphere.GENERATE_NORMALS, 20, app);
    }

    private final void create3DLink(int i, Transform3D transform)
    {
     
        final Transform3D buf = new Transform3D();
        final TransformGroup tgGiunto;
        TransformGroup tg = new TransformGroup();
        

        // Elementi 3D
        final Cylinder braccioX, braccioZ, braccioPrism;
        final Sphere giuntoPrism, giuntoRot;

        giuntoPrism = createJoint(0.09f, giuntiAppearance(PRISMATIC));
        giuntoPrism.setCapability(Sphere.ENABLE_APPEARANCE_MODIFY);
        
        giuntoRot = createJoint(0.09f, giuntiAppearance(REVOLUTE));
        giuntoRot.setCapability(Sphere.ENABLE_APPEARANCE_MODIFY);
        
        braccioPrism = createZArm(new Point3d(0.0, 0.0, -getQMin().getElement(i)),getQMin().getElement(i) - getQMax().getElement(i),
                bracciAppearance(/*PolygonAttributes.POLYGON_LINE*/));

        // braccio lungo la direzione X secondo il parametro a
        braccioX = createXArm(a[i], bracciAppearance());

        // braccio lungo la direzione Z secondo il parametro d
        braccioZ = createZArm(new Point3d(a[i], 0.0, 0.0), d[i], bracciAppearance());

        tgGiunto = new TransformGroup(transform);
        tgGiunto.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
        tgGiunto.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
        tgGiunto.setCapability(TransformGroup.ALLOW_LOCAL_TO_VWORLD_READ);

        if((tipoGiunto[i] == REVOLUTE))
            if((a[i] != 0) || (d[i] != 0))
            {
                tgGiunto.addChild(giuntoRot);
                shapeGiunti.add(giuntoRot);
            }

        // vengono aggiunti i bracci solo se a o d sono diversi da zero
        if((a[i] != 0))
            tgGiunto.addChild(braccioX);
        if((d[i] != 0) && (tipoGiunto[i] == REVOLUTE))
            tgGiunto.addChild(braccioZ);

        // aggiunta del giunto e del braccio prismatico alla trasformazione
        // precedente
        if((tipoGiunto[i] == PRISMATIC))
        {
            if((i != 0))
            {
                final TransformGroup tgPrism;
                tgGiunto.addChild(braccioPrism);
                tgPrism = (TransformGroup) tgGiunto.cloneNode(true);
                tg = (TransformGroup) tgGiunti.get(i - 1);

                tgPrism.addChild(giuntoPrism);
                shapeGiunti.add(giuntoPrism);
                /*tgPrism.addChild(braccioPrism);*/
                tg.addChild(tgPrism);
            }
            /*
             * else { // tgGiunto.addChild(giuntoPrism); //
             * tgGiunto.addChild(braccioPrism); }
             */
        }

        // aggiunta della terna i (eccetto della terna 0)
        if((i != 0))
        {
            final TransformGroup tgTerna = (TransformGroup) tgGiunto.cloneNode(true);
            tg = (TransformGroup) tgGiunti.get(i - 1);
            tgTerna.addChild(new Axis(0.11f));
            tg.addChild(tgTerna);
        }

        // memorizzazione della trasformazione
        tgGiunti.add(i, tgGiunto);
        // Memorizzazione della trasformazione di riferimento
        tgGiunto.getTransform(buf);
        tsGiuntiRef.add(i, new Transform3D(buf));

    }

    /**
     * Creation of 3D model from DH table
     */
    private void create3DModel()
    {
        final Transform3D transform = new Transform3D();
        TransformGroup tg = new TransformGroup();
        final TransformGroup tgTernaN = new TransformGroup();
        final Axis nFrame = new Axis(0.25f);

        RMatrix4d M = new RMatrix4d();
        M.setIdentity();

       
        // Cancellazione del precedente modello 3D se esiste
        if((bgRobot != null))
        {
            bgRobot.detach();
        }

        bgRobot = new BranchGroup();
        bgRobot.setCapability(BranchGroup.ALLOW_DETACH);

        // Matrici delle trasformazioni omogenee
     
        for (int i = 0; i < nLinks; i++)
        {

            // nodo i
            transform.set(M);

            create3DLink(i, transform);

 /*           if(DEBUG)
                System.out.println("\ni=" + i + "\nM=" + M);
*/
            // calcolo della matrice di trasformazione omogenea (per 3D)
            M = (tipoGiunto[i] == REVOLUTE)? computeMatrix(a[i], alfa[i], d[i], 0.0):computeMatrix(a[i], alfa[i], 0.0, teta[i]);
           /*     
            if((tipoGiunto[i] == REVOLUTE))
                M = computeMatrix(a[i], alfa[i], d[i], 0);
            else
                M = computeMatrix(a[i], alfa[i], 0, teta[i]);
           */
        }
        // incollamento dei pezzi
        for (int i = 0; i < nLinks - 1; i++)
        {
            tg = (TransformGroup) tgGiunti.get(i);
            tg.addChild((TransformGroup) tgGiunti.get(i + 1));
        }

        // Aggiunta della Terna n
        transform.set(M);
        tgTernaN.setTransform(transform);
        tgTernaN.addChild(nFrame);
        tg = (TransformGroup) tgGiunti.get(nLinks - 1);
        tg.addChild(tgTernaN);

        bgRobot.addChild((TransformGroup) tgGiunti.get(0));

        // ottimizzazione
        bgRobot.compile();

        // impostazione delle posizioni
        for (int i = 0; i < nLinks; i++)
        {
            if((tipoGiunto[i] == REVOLUTE))
                setJointPosition(i, teta[i]);
            else
                setJointPosition(i, d[i]);
        }

    }
    /**
     * Compute homogeneous trasformation matrix from DH parameters
     * 
     * @param a
     *            Description of the Parameter
     * @param alfa
     *            Description of the Parameter
     * @param d
     *            Description of the Parameter
     * @param teta
     *            Description of the Parameter
     * @return Description of the Return Value
     */
    private RMatrix4d computeMatrix(double a_, double alfa_, double d_, double teta_)
    {
 /*       if(DEBUG)
        {
            System.out.println("a: " + a + "\nalfa: " + alfa + "\nd: " + d
                    + "\nteta: " + teta + "\n");
        }
*/
        final double ca = Math.cos(alfa_);
        final double ct = Math.cos(teta_);
        final double sa = Math.sin(alfa_);
        final double st = Math.sin(teta_);

        // matrice di trasformazione omogenea
        final RMatrix4d M = new RMatrix4d();
        M.setIdentity();

        // prima riga
        M.m00 = ct;
        M.m01 = -(st * ca);
        M.m02 = st * sa;
        M.m03 = a_ * ct;

        // seconda riga
        M.m10 = st;
        M.m11 = ct * ca;
        M.m12 = -(ct * sa);
        M.m13 = a_ * st;

        // terza riga
        M.m21 = sa;
        M.m22 = ca;
        M.m23 = d_;

/*        if(DEBUG)
        {
            System.out.println("A:\n" + M);
        }
*/
        return M;
    }

    /**
     * Impostazione della posizione dei giunti.
     * The offset is added;
     * @param i
     * @param newPos_
     */
    private void setJointPosition(int i, double newPos_)
    {
        /* if(newPos_ < this.getQMin().getElement(i) || newPos_ > this.getQMax().getElement(i))
            shapeGiunti.get(i).setAppearance(outOfRangeAppearance()); */
           
            
        final double newPos = newPos_ + getQOffset().getElement(i);
        final Transform3D tsGiunto;
        final Transform3D transform = new Transform3D();
        final TransformGroup tgGiunto;
        
        if((tipoGiunto[i] == REVOLUTE))
        {
            final Transform3D rotate = new Transform3D();
            // trasformazione di riferimento
            tsGiunto = (Transform3D) tsGiuntiRef.get(i);
            // rotazione
            rotate.rotZ(newPos);
            transform.mul(tsGiunto, rotate);
            // impostazione della rotazione
            tgGiunto = (TransformGroup) tgGiunti.get(i);
            tgGiunto.setTransform(transform);
        }
        else
        {
            final Transform3D translate = new Transform3D();
            // trasformazione di riferimento
            tsGiunto = (Transform3D) tsGiuntiRef.get(i);
            // traslazione
            translate.setTranslation(new RVector3d(0.0, 0.0, newPos));
            transform.mul(tsGiunto, translate);
            // impostazione della rotazione
            tgGiunto = (TransformGroup) tgGiunti.get(i);
            tgGiunto.setTransform(transform);
        }

    }
    private Appearance bracciAppearance(int tipo)
    {
        final Appearance bracciAppear = new Appearance();
        final Material material = new Material();
   
        // aspetto dei bracci del manipolatore
        final PolygonAttributes pa = new PolygonAttributes(tipo,
                PolygonAttributes.CULL_BACK, 0.0f);

        bracciAppear.setPolygonAttributes(pa);
        material.setDiffuseColor(ColorConstants.DYELLOW);
        material.setSpecularColor(ColorConstants.YELLOW);

        material.setShininess(50.0f);
        bracciAppear.setMaterial(material);

        return bracciAppear;
    }
    
    private Appearance bracciAppearance()
    {
        return bracciAppearance(PolygonAttributes.POLYGON_FILL);
    }

    /**
     * Description of the Method
     * 
     * @return Description of the Return Value
     */
    private Appearance giuntiAppearance(boolean tipoGiunto_)
    {
        final Appearance giuntiAppear = new Appearance();
        final Material material = new Material();
        
        final PolygonAttributes pa;
        // aspetto dei giunti del manipolatore
        pa = new PolygonAttributes(PolygonAttributes.POLYGON_FILL,
                PolygonAttributes.CULL_BACK, 0.0f);

        giuntiAppear.setPolygonAttributes(pa);

        if((tipoGiunto_ == PRISMATIC))
        {
            material.setDiffuseColor(ColorConstants.DBLUE);
        }
        else
        {
            material.setDiffuseColor(ColorConstants.BLACK);
        }
        material.setSpecularColor(ColorConstants.WHITE);
        material.setShininess(300.0f);
        giuntiAppear.setMaterial(material);

        return giuntiAppear;
    }
 /* 
    private Appearance outOfRangeAppearance()
    {
        final Appearance giuntiAppear = new Appearance();
        final Material material = new Material();
        
        final PolygonAttributes pa;
        // aspetto dei giunti del manipolatore
        pa = new PolygonAttributes(PolygonAttributes.POLYGON_FILL,
                PolygonAttributes.CULL_BACK, 0.0f);

        giuntiAppear.setPolygonAttributes(pa);

        material.setDiffuseColor(ColorConstants.RED);
        material.setSpecularColor(ColorConstants.WHITE);
        material.setShininess(300.0f);
        giuntiAppear.setMaterial(material);

        return giuntiAppear;
    }
    */
    /**
     * Gets the 4x4 transformation matrix of i-th joint.
     * 
     * @return null (the homogeneous matrix.)
     */
    public RMatrix4d getTransformMatrix(int i)
    {
        return null;
    }
    

}
